﻿//--------------------------------------------------
// Author: 张羽乔
// License: LGPL 3
//--------------------------------------------------

#include <math.h>
#include <float.h>
#include <stdint.h>
#include <assert.h>

//线性代数

struct PTVectorSIMD
{
	__m128 v;
};

struct PTQuaternionSIMD
{
	__m128 q;
};

struct PTDualQuaternionSIMD
{
	__m128 _0;
	__m128 _1;
};

struct PTMatrixSIMD
{
	__m128 r[4];
};

inline PTVectorSIMD PT_VECTORCALL PTVector3FLoad(const PTVector3F *pSource)
{
	__m128 vX = ::_mm_load_ss( &pSource->x );//x _ _ _
    __m128 vY = ::_mm_load_ss( &pSource->y );//y _ _ _
    __m128 vZ = ::_mm_load_ss( &pSource->z );//z _ _ _

	PTVectorSIMD vResult;
	vResult.v = ::_mm_unpacklo_ps(vX, vY);//x y _ _
	vResult.v = ::_mm_shuffle_ps(vResult.v, vZ, _MM_SHUFFLE(0, 0, 1, 0));//x y z _
	return vResult;
}

inline PTVectorSIMD PT_VECTORCALL PTVector3FLoadA(const PTVector3F *pSource)
{
	assert((reinterpret_cast<uintptr_t>(pSource) % 16U) == 0U);

	PTVectorSIMD vResult;
	vResult.v = ::_mm_load_ps(&pSource->x);
	return vResult;
}

inline void PT_VECTORCALL PTVector3FStore(PTVector3F* pDestination, PTVectorSIMD V)
{
	__m128 vY = ::_mm_shuffle_ps(V.v, V.v, _MM_SHUFFLE(0, 0, 0, 1));//y _ _ _
	__m128 vZ = ::_mm_shuffle_ps(V.v, V.v, _MM_SHUFFLE(0, 0, 0, 2));//z _ _ _
	_mm_store_ss(&pDestination->x, V.v);
	_mm_store_ss(&pDestination->y, vY);
	_mm_store_ss(&pDestination->z, vZ);
}

inline void PT_VECTORCALL PTVector3FStoreA(PTVector3F* pDestination, PTVectorSIMD V)
{
	assert((reinterpret_cast<uintptr_t>(pDestination) % 16U) == 0U);

	::_mm_storel_epi64(reinterpret_cast<__m128i*>(pDestination), ::_mm_castps_si128(V.v));
	::_mm_store_ss(&pDestination->z, ::_mm_shuffle_ps(V.v, V.v, _MM_SHUFFLE(2, 2, 2, 2)));
}

inline PTVectorSIMD PT_VECTORCALL PTVector3SIMDDot(PTVectorSIMD V1, PTVectorSIMD V2)
{
	//映射
	__m128 vTemp1 = _mm_mul_ps(V1.v, V2.v);
	//归约
	__m128 vTemp2 = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 0, 0, 2));//z _ _ _
	vTemp1 = ::_mm_add_ss(vTemp1, vTemp2);//x+z y _ _ 注意是ss而不是ps!!!
	vTemp2 = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 0, 0, 1));//y _ _ _
	vTemp1 = ::_mm_add_ss(vTemp1, vTemp2);//x+z+y _ _ _
	
	PTVectorSIMD vResult;
	vResult.v = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 0, 0, 0));//x+z+y+w x+z+y+w x+z+y+w x+z+y+w
	return vResult;
}

inline PTVectorSIMD PT_VECTORCALL PTVector3SIMDCross(PTVectorSIMD V1, PTVectorSIMD V2)
{
	PTVectorSIMD vResult;
	vResult.v = _mm_sub_ps(//y1z2-z1y2 z1x2-x1z2 x1y2-y1x2
		::_mm_mul_ps(//y1z2 z1x2 x1y2
			::_mm_shuffle_ps(V1.v, V1.v, _MM_SHUFFLE(0, 0, 2, 1)),//y1 z1 x1 _
			::_mm_shuffle_ps(V2.v, V2.v, _MM_SHUFFLE(0, 1, 0, 2))//z2 x2 y2 _
		),
		::_mm_mul_ps(//z1y2 x1z2 y1x2
			::_mm_shuffle_ps(V1.v, V1.v, _MM_SHUFFLE(0, 1, 0, 2)),//z1 x1 y1 _
			::_mm_shuffle_ps(V2.v, V2.v, _MM_SHUFFLE(0, 0, 2, 1))//y2 z2 x2 _
		)
	);
	return vResult;
}

inline PTVectorSIMD PT_VECTORCALL PTVector3SIMDNormalize(PTVectorSIMD V)
{
	PTVectorSIMD vLengthSquare = ::PTVector3SIMDDot(V, V);
	
	__m128 vLength = ::_mm_sqrt_ps(vLengthSquare.v);

	//零向量的方向是任意的！！！

	PTVectorSIMD vResult;
	vResult.v = ::_mm_div_ps(V.v, vLength);
	return vResult;
}

inline PTVectorSIMD PT_VECTORCALL PTVector3SIMDTransform(PTMatrixSIMD M, PTVectorSIMD V)
{
	alignas(16) float f0001[4] = { 0.0f ,0.0f, 0.0f, 1.0f };
	__m128 vf0001 = ::_mm_load_ps(f0001);

	alignas(16) uint32_t uSelect1110[4] = { 0XFFFFFFFFU ,0XFFFFFFFFU ,0XFFFFFFFFU ,0X00000000U };
	__m128 vSelect1110 = ::_mm_load_ps(reinterpret_cast<float *>(uSelect1110));

	__m128 v4 = ::_mm_or_ps(
		::_mm_and_ps(vSelect1110, V.v),//x y z _
		::_mm_andnot_ps(vSelect1110, vf0001)//_ _ _ 1
	);

	return ::PTVector4SIMDTransform(M, PTVectorSIMD{ v4 });
}

inline PTVectorSIMD PT_VECTORCALL PTVector4FLoad(const PTVector4F *pSource)
{
	PTVectorSIMD vResult;
	vResult.v = ::_mm_loadu_ps(&pSource->x);
	return vResult;
}

inline PTVectorSIMD PT_VECTORCALL PTVector4FLoadA(const PTVector4F *pSource)
{
	assert((reinterpret_cast<uintptr_t>(pSource) % 16U) == 0U);

	PTVectorSIMD vResult;
	vResult.v = ::_mm_load_ps(&pSource->x);
	return vResult;
}

inline void PT_VECTORCALL PTVector4FStore(PTVector4F* pDestination, PTVectorSIMD V)
{
	::_mm_storeu_ps(&pDestination->x, V.v);
}

inline void PT_VECTORCALL PTVector4FStoreA(PTVector4F* pDestination, PTVectorSIMD V)
{
	assert((reinterpret_cast<uintptr_t>(pDestination) % 16U) == 0U);

	::_mm_store_ps(&pDestination->x, V.v);
}

inline PTVectorSIMD PT_VECTORCALL PTVector4SIMDDot(PTVectorSIMD V1, PTVectorSIMD V2)
{
	//映射
	__m128 vTemp1 = _mm_mul_ps(V1.v, V2.v);
	//归约
	__m128 vTemp2 = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 0, 3, 2));//z w _ _
	vTemp1 = ::_mm_add_ps(vTemp1, vTemp2);//x+z y+w _ _ 
	vTemp2 = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 0, 0, 1));//y+w _ _ _
	vTemp1 = ::_mm_add_ss(vTemp1, vTemp2);//x+z+y+w _ _ _ 注意是ss而不是ps!!!

	PTVectorSIMD vResult;
	vResult.v = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 0, 0, 0));//x+z+y+w x+z+y+w x+z+y+w x+z+y+w
	return vResult;
}


inline PTVectorSIMD PT_VECTORCALL PTVector4SIMDTransform(PTMatrixSIMD M, PTVectorSIMD V)
{
	PTMatrixSIMD mTemp;
	//映射
	mTemp.r[0] = ::_mm_mul_ps(M.r[0], V.v);
	mTemp.r[1] = ::_mm_mul_ps(M.r[1], V.v);
	mTemp.r[2] = ::_mm_mul_ps(M.r[2], V.v);
	mTemp.r[3] = ::_mm_mul_ps(M.r[3], V.v);

	//归约
	mTemp = ::PTMatrix4x4SIMDTranspose(mTemp);
	PTVectorSIMD vResult;
	vResult.v = ::_mm_add_ps(::_mm_add_ps(mTemp.r[0], mTemp.r[1]), ::_mm_add_ps(mTemp.r[2], mTemp.r[3]));

	return vResult;
}

inline PTVectorSIMD PT_VECTORCALL PTVectorSIMDNegate(PTVectorSIMD V)
{
	__m128 vZero = ::_mm_setzero_ps();
	PTVectorSIMD vResult;
	vResult.v = ::_mm_sub_ps(vZero, V.v);
	return vResult;
}

inline PTQuaternionSIMD PT_VECTORCALL PTQuaternionSIMDRotationAxis(PTVectorSIMD Axis, float Angle)
{

	PTVectorSIMD NormalAxis = ::PTVector3SIMDNormalize(Axis);
	return PTQuaternionSIMD{ ::PTQuaternionSIMDRotationNormalAxis(NormalAxis, Angle) };
}

inline PTQuaternionSIMD PT_VECTORCALL PTQuaternionSIMDRotationNormalAxis(PTVectorSIMD NormalAxis, float Angle)
{
	float fHalfAngle = Angle * 0.5f;
	float fSin = ::sinf(fHalfAngle);
	float fCos = ::cosf(fHalfAngle);

	__m128 vSin = ::_mm_load_ps1(&fSin);
	__m128 vCos = ::_mm_load_ps1(&fCos);

	alignas(16) uint32_t uSelect1110[4] = { 0XFFFFFFFFU ,0XFFFFFFFFU ,0XFFFFFFFFU ,0X00000000U };
	__m128 vSelect1110 = ::_mm_load_ps(reinterpret_cast<float *>(uSelect1110));

	return PTQuaternionSIMD{ ::_mm_or_ps(
		::_mm_and_ps(vSelect1110, ::_mm_mul_ps(NormalAxis.v, vSin)),//CinU/2*(Nx,Ny,Nz) _
		::_mm_andnot_ps(vSelect1110, vCos)//_ _ _ CosU/2
	) };
}

inline PTDualQuaternionSIMD PT_VECTORCALL PTQuaternionRigidMatrix(PTMatrixSIMD M)
{

}

inline void PT_VECTORCALL PTMatrix4x4FStore(PTMatrix4x4F* pDestination, PTMatrixSIMD M)
{
	::_mm_storeu_ps(&pDestination->r[0].x, M.r[0]);
	::_mm_storeu_ps(&pDestination->r[1].x, M.r[1]);
	::_mm_storeu_ps(&pDestination->r[2].x, M.r[2]);
	::_mm_storeu_ps(&pDestination->r[3].x, M.r[3]);
}

inline void PT_VECTORCALL PTMatrix4x4FStoreA(PTMatrix4x4F* pDestination, PTMatrixSIMD M)
{
	assert((reinterpret_cast<uintptr_t>(&pDestination->r[0].x) % 16U) == 0U);
	assert((reinterpret_cast<uintptr_t>(&pDestination->r[1].x) % 16U) == 0U);
	assert((reinterpret_cast<uintptr_t>(&pDestination->r[2].x) % 16U) == 0U);
	assert((reinterpret_cast<uintptr_t>(&pDestination->r[3].x) % 16U) == 0U);

	::_mm_store_ps(&pDestination->r[0].x, M.r[0]);
	::_mm_store_ps(&pDestination->r[1].x, M.r[1]);
	::_mm_store_ps(&pDestination->r[2].x, M.r[2]);
	::_mm_store_ps(&pDestination->r[3].x, M.r[3]);
}

inline PTMatrixSIMD PT_VECTORCALL PTMatrix4x4SIMDTranspose(PTMatrixSIMD M)
{
	__m128 vTemp1 = ::_mm_shuffle_ps(M.r[0], M.r[1], _MM_SHUFFLE(1, 0, 1, 0));//x0 y0 x1 y1
	__m128 vTemp2 = ::_mm_shuffle_ps(M.r[2], M.r[3], _MM_SHUFFLE(1, 0, 1, 0));//x2 y2 x3 y3
	__m128 vTemp3 = ::_mm_shuffle_ps(M.r[0], M.r[1], _MM_SHUFFLE(3, 2, 3, 2));//z0 w0 z1 w1
	__m128 vTemp4 = ::_mm_shuffle_ps(M.r[2], M.r[3], _MM_SHUFFLE(3, 2, 3, 2));//z2 w2 z3 w3

	PTMatrixSIMD mResult;
	mResult.r[0] = ::_mm_shuffle_ps(vTemp1, vTemp2, _MM_SHUFFLE(2, 0, 2, 0));//x0 x1 x2 x3
	mResult.r[1] = ::_mm_shuffle_ps(vTemp1, vTemp2, _MM_SHUFFLE(3, 1, 3, 1));//y0 y1 y2 y3
	mResult.r[2] = ::_mm_shuffle_ps(vTemp3, vTemp4, _MM_SHUFFLE(2, 0, 2, 0));//z0 z1 z2 z3
	mResult.r[3] = ::_mm_shuffle_ps(vTemp3, vTemp4, _MM_SHUFFLE(3, 1, 3, 1));//w0 w1 w2 w3
	return mResult;
}

inline PTMatrixSIMD PT_VECTORCALL PTMatrix4x4SIMDTranslation(float OffsetX, float OffsetY, float OffsetZ)
{
	PTMatrixSIMD mResult;
	alignas(16) float f1000[4] = { 1.0f ,0.0f ,0.0f ,OffsetX };
	alignas(16) float f0100[4] = { 0.0f ,1.0f ,0.0f ,OffsetY };
	alignas(16) float f0010[4] = { 0.0f ,0.0f ,1.0f ,OffsetZ };
	alignas(16) float f0001[4] = { 0.0f ,0.0f, 0.0f, 1.0f };
	mResult.r[0] = ::_mm_load_ps(f1000);
	mResult.r[1] = ::_mm_load_ps(f0100);
	mResult.r[2] = ::_mm_load_ps(f0010);
	mResult.r[3] = ::_mm_load_ps(f0001);
	return mResult;
}

inline PTMatrixSIMD PT_VECTORCALL PTMatrix4x4SIMDRotationQuaternion(PTQuaternionSIMD Q)
{
	//QaQa-QbQb-QcQc+QdQd	2QaQb-QcQd				2QaQc+QbQd
	//2QaQb+2QcQd			-QaQa+QbQb-QcQc+QdQd	-2QaQd+2QbQc
	//2QaQc-2QbQd			2QaQd+2QbQc				-QaQa-QbQb+QcQc+QdQd

	//由于Qa*Qa+Qb*Qb+Qc*Qc+Qd*Qd=1
	//1-2QbQb-2QcQc		2QaQb-QcQd			2QaQc+QbQd
	//2QaQb+2QcQd		1-2QaQa-2QcQc		-2QaQd+2QbQc
	//2QaQc-2QbQd		2QaQd+2QbQc			1-2QaQa-2QbQb


	__m128 v2q = ::_mm_add_ps(Q.q, Q.q);//2Qa 2Qb 2Qc 2Qd

	__m128 vTemp1 = ::_mm_mul_ps(v2q, Q.q);//2QaQa 2QbQb 2QcQc 2QdQd
	__m128 vTemp2 = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 0, 0, 1));//2QbQb 2QaQa 2QaQa _
	vTemp1 = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 1, 2, 2));//2QcQc 2QcQc 2QbQb _

	alignas(16) float f1110[4] = { 1.0f ,1.0f ,1.0f ,0.0f };
	__m128 v1110 = ::_mm_load_ps(f1110);

	__m128 vSquare = ::_mm_sub_ps(v1110, ::_mm_add_ps(vTemp2, vTemp1));//1-2QbQb-2QcQc 1-2QaQa2QcQc 1-2QaQa-2QbQb _

	vTemp1 = ::_mm_shuffle_ps(v2q, v2q, _MM_SHUFFLE(0, 1, 0, 0));//2Qa 2Qa 2Qb _
	vTemp2 = ::_mm_shuffle_ps(Q.q, Q.q, _MM_SHUFFLE(0, 2, 2, 1));//Qb Qc Qc _
	__m128 vABACBC = ::_mm_mul_ps(vTemp1, vTemp2);//2QaQb 2QaQc 2QbQc _

	vTemp1 = ::_mm_shuffle_ps(v2q, v2q, _MM_SHUFFLE(0, 0, 1, 2));//2Qc 2Qb 2Qa _
	vTemp2 = ::_mm_shuffle_ps(Q.q, Q.q, _MM_SHUFFLE(0, 3, 3, 3));//Qd Qd Qd _
	__m128 vQAQBQC = ::_mm_mul_ps(vTemp1, vTemp2);//2QcQd 2QbQd 2QaQd _

	__m128 vAdd = ::_mm_add_ps(vABACBC, vQAQBQC);//2QaQb+2QcQd 2QaQc+2QbQd 2QbQc+2QaQd _
	__m128 vSub = ::_mm_sub_ps(vABACBC, vQAQBQC);//2QaQb-2QcQd 2QaQc-2QbQd 2QbQc-2QaQd _

	__m128 v12 = ::_mm_shuffle_ps(vAdd, vSub, _MM_SHUFFLE(2, 0, 0, 1));//2QaQc+2QbQd 2QaQb+2QcQd 2QaQb-2QcQd 2QbQc-2QaQd
	__m128 v34 = ::_mm_shuffle_ps(vAdd, vSub, _MM_SHUFFLE(0, 1, 0, 2));//2QbQc+2QaQd _           2QaQc-2QbQd _

	PTMatrixSIMD mResult;
	mResult.r[0] = ::_mm_shuffle_ps(vSquare, v12, _MM_SHUFFLE(0, 2, 0, 0));//1-2QbQb-2QcQc _ 2QaQb-2QcQd 2QaQc+2QbQd
	mResult.r[0] = ::_mm_shuffle_ps(mResult.r[0], mResult.r[0], _MM_SHUFFLE(0, 3, 2, 0));//1-2QbQb-2QcQc 2QaQb-2QcQd 2QaQc+2QbQd _
	mResult.r[1] = ::_mm_shuffle_ps(vSquare, v12, _MM_SHUFFLE(3, 1, 0, 1));//1-2QaQa-2QcQc _ 2QaQb+2QcQd 2QbQc-2QaQd
	mResult.r[1] = ::_mm_shuffle_ps(mResult.r[1], mResult.r[1], _MM_SHUFFLE(0, 3, 0, 2));//2QaQb+2QcQd 1-2QaQa-2QcQc 2QbQc-2QaQd _
	mResult.r[2] = ::_mm_shuffle_ps(vSquare, v34, _MM_SHUFFLE(0, 2, 0, 2));//1-2QaQa-2QbQb _ 2QaQc-2QbQd 2QbQc+2QaQd
	mResult.r[2] = ::_mm_shuffle_ps(mResult.r[2], mResult.r[2], _MM_SHUFFLE(0, 0, 3, 2));//2QaQc-2QbQd 2QbQc+2QaQd 1-2QaQa-2QbQb _

	alignas(16) uint32_t uSelect1110[4] = { 0XFFFFFFFFU ,0XFFFFFFFFU ,0XFFFFFFFFU ,0X00000000U };
	__m128 vSelect1110 = ::_mm_load_ps(reinterpret_cast<float *>(uSelect1110));

	mResult.r[0] = ::_mm_and_ps(vSelect1110, mResult.r[0]);
	mResult.r[1] = ::_mm_and_ps(vSelect1110, mResult.r[1]);
	mResult.r[2] = ::_mm_and_ps(vSelect1110, mResult.r[2]);

	alignas(16) float f0001[4] = { 0.0f ,0.0f ,0.0f ,1.0f };
	mResult.r[3] = ::_mm_load_ps(f0001);

	return mResult;
}

inline PTMatrixSIMD PT_VECTORCALL PTMatrix4x4SIMDLookToRH(PTVectorSIMD EyePosition, PTVectorSIMD EyeDirection, PTVectorSIMD UpDirection, PTMatrixSIMD *pInverse)
{
	alignas(16) uint32_t uSelect1110[4] = { 0XFFFFFFFFU ,0XFFFFFFFFU ,0XFFFFFFFFU ,0X00000000U };
	__m128 vSelect1110 = ::_mm_load_ps(reinterpret_cast<float *>(uSelect1110));

	alignas(16) float f0001[4] = { 0.0f ,0.0f ,0.0f ,1.0f };
	__m128 v0001 = ::_mm_load_ps(f0001);

	PTVectorSIMD NegEyeDirection = ::PTVectorSIMDNegate(EyeDirection);
	PTVectorSIMD AZ = ::PTVector3SIMDNormalize(NegEyeDirection);
	PTVectorSIMD AX = ::PTVector3SIMDNormalize(::PTVector3SIMDCross(UpDirection, AZ));
	PTVectorSIMD AY = ::PTVector3SIMDCross(AZ, AX);

	if (pInverse != NULL)
	{
		//基变换
		//|AXx AYx AZx 0|
		//|AXy AYy AZy 0|
		//|AXz AYz AYz 0|
		//|0   0   0   1|

		//平移
		//|1 0 0 Px|
		//|0 1 0 Py|
		//|0 0 1 Pz|
		//|0 0 0 1 |

		//平移×基变换
		//|AXx AYx AZx Px|
		//|AXy AYy AZy Py|
		//|AXz AYz AYz Pz|
		//|0   0   0   1 |

		PTMatrixSIMD mInverse;
		mInverse.r[0] = ::_mm_and_ps(vSelect1110, AX.v);//AXx AXy AXz 0
		mInverse.r[1] = ::_mm_and_ps(vSelect1110, AY.v);//AYx AYy AYz 0
		mInverse.r[2] = ::_mm_and_ps(vSelect1110, AZ.v);//AZx AZy AZz 0
		mInverse.r[3] = ::_mm_or_ps(
			::_mm_and_ps(vSelect1110, EyePosition.v),//Px Py Pz _
			::_mm_andnot_ps(vSelect1110, v0001)//_ _ _ 1
		);

		(*pInverse) = PTMatrix4x4SIMDTranspose(mInverse);
	}

	//正交阵的逆为正交阵的转置
	//|AXx AXy AXz 0|
	//|AYx AYy AYz 0|
	//|AZx AZy AZz 0|
	//|0   0   0   1|

	//平移
	//|1 0 0 -Px|
	//|0 1 0 -Py|
	//|0 0 1 -Pz|
	//|0 0 0 1 |
	PTVectorSIMD NegEyePosition = ::PTVectorSIMDNegate(EyePosition);
	
	//相乘
	PTVectorSIMD DX = ::PTVector3SIMDDot(AX, NegEyePosition);
	PTVectorSIMD DY = ::PTVector3SIMDDot(AY, NegEyePosition);
	PTVectorSIMD DZ = ::PTVector3SIMDDot(AZ, NegEyePosition);

	PTMatrixSIMD mResult;
	mResult.r[0] = ::_mm_or_ps(
		::_mm_and_ps(vSelect1110, AX.v),//RX x y z _
		::_mm_andnot_ps(vSelect1110, DX.v)//DX _ _ _ w
	);
	mResult.r[1] = ::_mm_or_ps(
		::_mm_and_ps(vSelect1110, AY.v),//RY x y z _
		::_mm_andnot_ps(vSelect1110, DY.v)//DY _ _ _ w
	);
	mResult.r[2] = ::_mm_or_ps(
		::_mm_and_ps(vSelect1110, AZ.v),//RZ x y z _
		::_mm_andnot_ps(vSelect1110, DZ.v)//DZ _ _ _ w
	);
	mResult.r[3] = v0001;

	return mResult;
}


inline PTMatrixSIMD PT_VECTORCALL PTMatrix4x4SIMDPerspectiveFovRH(float FovAngleY, float AspectHByW, float NearZ, float FarZ, PTMatrixSIMD *pInverse)
{
	//投影面为z=1.0f
	float HalfViewHeight = ::tanf(0.5f * FovAngleY);
	float HalfViewWidth = HalfViewHeight * AspectHByW;

	return ::PTMatrix4x4SIMDPerspectiveRH(HalfViewWidth*2.0f, HalfViewHeight*2.0f, NearZ, FarZ, pInverse);
}

inline PTMatrixSIMD PT_VECTORCALL PTMatrix4x4SIMDPerspectiveRH(float ViewWidth, float ViewHeight, float NearZ, float FarZ, PTMatrixSIMD *pInverse)
{
	//Nvidia Reversed-Z Trick
	//http://developer.nvidia.com/content/depth-precision-visualized

	// _  0  0  0
	// 0  _  0  0
	// 0  0  b  a
	// 0  0 -1  0

	//Inverse
	//1/_  0   0   0
	// 0  1/_  0   0
	// 0   0   0   -1
	// 0   0  1/a b/a

	alignas(16) uint32_t uSelect0100[4] = { 0x00000000 ,0xFFFFFFFF ,0x00000000 ,0x00000000 };
	__m128 vSelect0100 = ::_mm_load_ps(reinterpret_cast<float *>(uSelect0100));
	
	alignas(16) float f000_1[4] = { 0.0f, 0.0f, 0.0f , -1.0f };
	__m128 v000_1 = ::_mm_load_ps(f000_1);

	if (pInverse)
	{
		float _1Da = (FarZ - NearZ) / FarZ;//确保FarZ趋向于正无穷大时的正确性
		_1Da /= NearZ;
		float _bDa = 1.0f / FarZ;

		alignas(16) float fTempInverse[4] = { ViewWidth / 2.0f,ViewHeight / 2.0f ,_1Da,_bDa };
		__m128 vTempInverse = ::_mm_load_ps(fTempInverse);//Width/2 Height/2 1/a b/a

		PTMatrixSIMD mInverse;
		//r0
		mInverse.r[0] = ::_mm_setzero_ps();
		mInverse.r[0] = ::_mm_move_ss(mInverse.r[0], vTempInverse);//Width/2 0 0 0
		//r1
		mInverse.r[1] = ::_mm_and_ps(vSelect0100, vTempInverse);//0 Height/2 0 0
		//r2
		mInverse.r[2] = v000_1;
		//r3
		mInverse.r[3] = ::_mm_setzero_ps();
		mInverse.r[3] = ::_mm_shuffle_ps(mInverse.r[3], vTempInverse, _MM_SHUFFLE(3, 2, 0, 0));//0 0 1/a b/a

		(*pInverse) = mInverse;
	}

	float b = NearZ / (FarZ - NearZ);
	float a = FarZ / (FarZ - NearZ);//确保FarZ趋向于正无穷大时的正确性
	a *= NearZ;

	alignas(16) float fTemp[4] = { 2.0f / ViewWidth,2.0f / ViewHeight,b,a };
	__m128 vTemp = ::_mm_load_ps(fTemp);//2/Width 2/Height b a

	PTMatrixSIMD mResult;
	//r0
	mResult.r[0] = ::_mm_setzero_ps();
	mResult.r[0] = ::_mm_move_ss(mResult.r[0], vTemp);//2/Width 0 0 0
	//r1
	mResult.r[1] = ::_mm_and_ps(vSelect0100, vTemp);//0 2/Height 0 0
	//r2
	mResult.r[2] = ::_mm_setzero_ps();
	mResult.r[2] = ::_mm_shuffle_ps(mResult.r[2], vTemp, _MM_SHUFFLE(3, 2, 0, 0));//0 0 b a
	//r3
	mResult.r[3] = ::_mm_shuffle_ps(v000_1, v000_1, _MM_SHUFFLE(2, 3, 1, 0));//0 0 -1 0

	return mResult;
}

inline PTMatrixSIMD PT_VECTORCALL PTMatrix4x4SIMDOrthographicRH(float ViewWidth, float ViewHeight, float NearZ, float FarZ)
{
	//Nvidia Reversed-Z Trick
	//http://developer.nvidia.com/content/depth-precision-visualized

	// _ 0 0 0
	// 0 _ 0 0
	// 0 0 b a
	// 0 0 0 1
	float b = 1.0f / (FarZ - NearZ);
	float a = FarZ / (FarZ - NearZ);//确保FarZ趋向于正无穷大时的正确性
	alignas(16) float fValues[4] = { 2.0f / ViewWidth,2.0f / ViewHeight,b,a };
	__m128 vValues = ::_mm_load_ps(fValues);//2/Width 2/Height b a

	PTMatrixSIMD mResult;
	//r0
	mResult.r[0] = ::_mm_setzero_ps();
	mResult.r[0] = ::_mm_move_ss(mResult.r[0], vValues);//2/Width 0 0 0
	//r1
	alignas(16) uint32_t uSelect0100[4] = { 0x00000000 ,0xFFFFFFFF ,0x00000000 ,0x00000000 };
	__m128 vSelect0100 = ::_mm_load_ps(reinterpret_cast<float *>(uSelect0100));
	mResult.r[1] = ::_mm_and_ps(vSelect0100, vValues);//0 2/Height 0 0
	//r2
	mResult.r[2] = ::_mm_setzero_ps();
	mResult.r[2] = ::_mm_shuffle_ps(mResult.r[2], vValues, _MM_SHUFFLE(3, 2, 0, 0));//0 0 b a
	//r3
	alignas(16) float fSelect0010[4] = { 0.0f, 0.0f, 0.0f , 1.0f };
	mResult.r[3] = ::_mm_load_ps(fSelect0010);//0 0 0 1
	return mResult;
}


inline PTMatrixSIMD PT_VECTORCALL PTMatrix4x4SIMDMultiply(PTMatrixSIMD M1, PTMatrixSIMD M2)
{
	PTMatrixSIMD mResult;
	__m128 vR = M1.r[0];//a00 a01 a02 a03
	__m128 vX = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(0, 0, 0, 0));//a00 a00 a00 a00
	__m128 vY = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(1, 1, 1, 1));//a01 a01 a01 a01
	__m128 vZ = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(2, 2, 2, 2));//a02 a02 a02 a02
	__m128 vW = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(3, 3, 3, 3));//a03 a03 a03 a03
	
	//计算第一行
	//映射
	vX = ::_mm_mul_ps(vX, M2.r[0]);//a00*b00 a00*b01 a00*b02 a00*b03
	vY = ::_mm_mul_ps(vY, M2.r[1]);//a01*b10 a01*b11 a01*b12 a01*b13
	vZ = ::_mm_mul_ps(vZ, M2.r[2]);//a02*b20 a02*b21 a02*b22 a02*b23
	vW = ::_mm_mul_ps(vW, M2.r[3]);//a03*b30 a03*b31 a03*b32 a03*b33
	//归约
	mResult.r[0] = ::_mm_add_ps(::_mm_add_ps(vX, vZ), ::_mm_add_ps(vY, vW));//a00*b00+a01*b10+a02*b20+a03*b30 a00*b01+a01*b11+a02*b21+a03*b31 a00*b02+a01*b12+a02*b22+a03*b32 a00*b03+a01*b13+a02*b23+a03*b33
	
	//计算其它三行
	vR = M1.r[1];
	vX = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(0, 0, 0, 0));
	vY = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(1, 1, 1, 1));
	vZ = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(2, 2, 2, 2));
	vW = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(3, 3, 3, 3));
	vX = ::_mm_mul_ps(vX, M2.r[0]);
	vY = ::_mm_mul_ps(vY, M2.r[1]);
	vZ = ::_mm_mul_ps(vZ, M2.r[2]);
	vW = ::_mm_mul_ps(vW, M2.r[3]);
	mResult.r[1] = ::_mm_add_ps(::_mm_add_ps(vX, vZ), ::_mm_add_ps(vY, vW));
	
	vR = M1.r[2];
	vX = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(0, 0, 0, 0));
	vY = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(1, 1, 1, 1));
	vZ = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(2, 2, 2, 2));
	vW = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(3, 3, 3, 3));
	vX = ::_mm_mul_ps(vX, M2.r[0]);
	vY = ::_mm_mul_ps(vY, M2.r[1]);
	vZ = ::_mm_mul_ps(vZ, M2.r[2]);
	vW = ::_mm_mul_ps(vW, M2.r[3]);
	mResult.r[2] = ::_mm_add_ps(::_mm_add_ps(vX, vZ), ::_mm_add_ps(vY, vW));

	vR = M1.r[3];
	vX = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(0, 0, 0, 0));
	vY = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(1, 1, 1, 1));
	vZ = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(2, 2, 2, 2));
	vW = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(3, 3, 3, 3));
	vX = ::_mm_mul_ps(vX, M2.r[0]);
	vY = ::_mm_mul_ps(vY, M2.r[1]);
	vZ = ::_mm_mul_ps(vZ, M2.r[2]);
	vW = ::_mm_mul_ps(vW, M2.r[3]);
	mResult.r[3] = ::_mm_add_ps(::_mm_add_ps(vX, vZ), ::_mm_add_ps(vY, vW));

	return mResult;
}

//相交测试

struct PTFrustumSIMD //视锥体（四棱台）
{
	__m128 Plane[6];
};

struct PTSphereSIMD
{

	__m128 Center;
	__m128 Radius;
};

struct PTAABSIMD
{

	__m128 Center;
	__m128 HalfDiagonal;
};

struct PTOBSIMD
{

	__m128 Center;
	__m128 HalfDiagonal;
	__m128 Axis[3];
};

//向量(a,b,c,d)表示平面ax + by + cz + d = 0
//点P(px,py,pc)到平面上任意一点在平面法向量(a,b,c)上的投影为(a * xp + b * yp + c * pc + d) / 根号(a^2 + b^2 + c^2)，特别地，当平面法向量为单位向量时，根号(a^2 + b^2 + c^2)为1，上式即为(px,py,pc,1)点积(a,b,c,d)
//可根据投影与0的大小关系确定点P在平面的哪一侧

inline __m128 PT_VECTORCALL PTSIMDInternalPlaneNormalize(__m128 P)
{
	//计算(P.x,P.y,P.z)的长度
	PTVectorSIMD vLengthSq = ::PTVector3SIMDDot(PTVectorSIMD{ P }, PTVectorSIMD{ P });
	__m128 vLength = _mm_sqrt_ps(vLengthSq.v);

	//零向量的方向是任意的！！！
	__m128 vResult = _mm_div_ps(P, vLength);

	return vResult;
}

inline PTFrustumSIMD PT_VECTORCALL PTFrustumSIMDLoadRH(PTMatrixSIMD ViewProjection)
{
	//Gil Gribb  Klaus Hartmann,"Fast Extraction of Viewing Frustum Planes from the WorldView-Projection Matrix", 06/15/2001
	//http://gamedevs.org/uploads/fast-extraction-viewing-frustum-planes-from-world-view-projection-matrix.pdf

	PTFrustumSIMD F;

	//Near
	F.Plane[0] = ::_mm_sub_ps(ViewProjection.r[3], ViewProjection.r[2]);
	F.Plane[0] = ::PTSIMDInternalPlaneNormalize(F.Plane[0]);

	//Far
	F.Plane[1] = ViewProjection.r[2];
	F.Plane[1] = ::PTSIMDInternalPlaneNormalize(F.Plane[1]);

	//Right
	F.Plane[2]= ::_mm_sub_ps(ViewProjection.r[3], ViewProjection.r[0]);
	F.Plane[2] = ::PTSIMDInternalPlaneNormalize(F.Plane[2]);

	//Left
	F.Plane[3] = ::_mm_add_ps(ViewProjection.r[3], ViewProjection.r[0]);
	F.Plane[3] = ::PTSIMDInternalPlaneNormalize(F.Plane[3]);

	//Top
	F.Plane[4] = ::_mm_sub_ps(ViewProjection.r[3], ViewProjection.r[1]);
	F.Plane[4] = ::PTSIMDInternalPlaneNormalize(F.Plane[4]);

	//Bottom
	F.Plane[5] = ::_mm_add_ps(ViewProjection.r[3], ViewProjection.r[1]);
	F.Plane[5] = ::PTSIMDInternalPlaneNormalize(F.Plane[5]);

	return F;
}

inline void PT_VECTORCALL PTFrustumFStore(PTFrustumF *pDestination, PTFrustumSIMD F)
{
	::_mm_storeu_ps(&pDestination->Plane[0].x, F.Plane[0]);
	::_mm_storeu_ps(&pDestination->Plane[1].x, F.Plane[1]);
	::_mm_storeu_ps(&pDestination->Plane[2].x, F.Plane[2]);
	::_mm_storeu_ps(&pDestination->Plane[3].x, F.Plane[3]);
	::_mm_storeu_ps(&pDestination->Plane[4].x, F.Plane[4]);
	::_mm_storeu_ps(&pDestination->Plane[5].x, F.Plane[5]);
}

inline void PT_VECTORCALL PTFrustumFStoreA(PTFrustumF *pDestination, PTFrustumSIMD F)
{
	assert((reinterpret_cast<uintptr_t>(&pDestination->Plane[0].x) % 16U) == 0U);
	assert((reinterpret_cast<uintptr_t>(&pDestination->Plane[1].x) % 16U) == 0U);
	assert((reinterpret_cast<uintptr_t>(&pDestination->Plane[2].x) % 16U) == 0U);
	assert((reinterpret_cast<uintptr_t>(&pDestination->Plane[3].x) % 16U) == 0U);
	assert((reinterpret_cast<uintptr_t>(&pDestination->Plane[4].x) % 16U) == 0U);
	assert((reinterpret_cast<uintptr_t>(&pDestination->Plane[5].x) % 16U) == 0U);

	::_mm_store_ps(&pDestination->Plane[0].x, F.Plane[0]);
	::_mm_store_ps(&pDestination->Plane[1].x, F.Plane[1]);
	::_mm_store_ps(&pDestination->Plane[2].x, F.Plane[2]);
	::_mm_store_ps(&pDestination->Plane[3].x, F.Plane[3]);
	::_mm_store_ps(&pDestination->Plane[4].x, F.Plane[4]);
	::_mm_store_ps(&pDestination->Plane[5].x, F.Plane[5]);
}

inline PTSphereSIMD PT_VECTORCALL PTSphereSIMDLoad(PTVector3F *pCenter, float Radius)
{
	PTSphereSIMD S;
	S.Center = ::_mm_set_ps(1.0f, pCenter->z, pCenter->y, pCenter->x);
	S.Radius = ::_mm_set_ps1(Radius);
	return S;
}

inline PTAABSIMD PT_VECTORCALL PTAABSIMDLoad(PTVector3F *pMin, PTVector3F *pMax)
{
	PTAABSIMD AAB;
	__m128 vMin= ::_mm_set_ps(1.0f, pMin->z, pMin->y, pMin->x);
	__m128 vMax = ::_mm_set_ps(1.0f, pMax->z, pMax->y, pMax->x);
	float fHalf = 0.5f;
	__m128 vHalf = ::_mm_load_ps1(&fHalf);
	AAB.Center = ::_mm_add_ps(vMin, vMax);
	AAB.Center = ::_mm_mul_ps(AAB.Center, vHalf);
	AAB.HalfDiagonal = ::_mm_sub_ps(vMax, vMin);
	AAB.HalfDiagonal = ::_mm_mul_ps(AAB.HalfDiagonal, vHalf);
	return AAB;
}

inline void PT_VECTORCALL PTSIMDInternalPlaneSphereIntersect(__m128 P, PTSphereSIMD S, __m128 &Inside, __m128 &OutSide)
{
	//球心到平面上任意一点在平面法向量上的投影
	PTVectorSIMD vD = ::PTVector4SIMDDot(PTVectorSIMD{ P }, PTVectorSIMD{ S.Center });

	//与PTSIMDFrustumLoadRH一致
	Inside = ::_mm_cmpgt_ps(vD.v, S.Radius);
	OutSide = ::_mm_cmplt_ps(vD.v, ::PTVectorSIMDNegate(PTVectorSIMD{ S.Radius }).v);
}

inline PTIntersectionType PT_VECTORCALL PTFrustumSIMDSphereSIMDIntersect(PTFrustumSIMD F, PTSphereSIMD S)
{
	__m128 AnyOutside;
	__m128 AllInside;

	__m128 OutSide;
	__m128 Inside;

	::PTSIMDInternalPlaneSphereIntersect(F.Plane[0], S, Inside, OutSide);
	AnyOutside = OutSide;
	AllInside = Inside;

	::PTSIMDInternalPlaneSphereIntersect(F.Plane[1], S, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	::PTSIMDInternalPlaneSphereIntersect(F.Plane[2], S, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	::PTSIMDInternalPlaneSphereIntersect(F.Plane[3], S, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	::PTSIMDInternalPlaneSphereIntersect(F.Plane[4], S, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	::PTSIMDInternalPlaneSphereIntersect(F.Plane[5], S, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	if (::_mm_movemask_ps(AnyOutside))//==0XF
	{
		return PTDISJOINT;
	}
	else if (::_mm_movemask_ps(AllInside))//==0XF
	{
		return PTCONTAIN;
	}
	else
	{
		return PTINTERSECT;
	}
}

inline void PT_VECTORCALL PTSIMDInternalPlaneAABIntersect(__m128 P, PTAABSIMD AAB, __m128 &Inside, __m128 &OutSide)
{
	//长方体中心到平面上任意一点在平面法向量上的投影
	PTVectorSIMD vD = ::PTVector4SIMDDot(PTVectorSIMD{ P }, PTVectorSIMD{ AAB.Center });

	//长方体8个半对角线在平面法向量上的投影的最大值
	//映射
	__m128 vTemp1 = _mm_mul_ps(P, AAB.HalfDiagonal);//逐分量相乘
	__m128 vTemp2 = _mm_setzero_ps();
	vTemp2 = _mm_sub_ps(vTemp2, vTemp1);
	vTemp1 = _mm_max_ps(vTemp1, vTemp2);//取绝对值
	//归约
	vTemp2 = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 0, 0, 2));//z _ _ _
	vTemp1 = ::_mm_add_ss(vTemp1, vTemp2);//x+z y _ _ 注意是ss而不是ps!!!
	vTemp2 = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 0, 0, 1));//y _ _ _
	vTemp1 = ::_mm_add_ss(vTemp1, vTemp2);//x+z+y _ _ _

	vTemp1 = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 0, 0, 0));//x+z+y x+z+y x+z+y x+z+y

	//与PTSIMDFrustumLoadRH一致
	Inside = ::_mm_cmpgt_ps(vD.v, vTemp1);
	OutSide = ::_mm_cmplt_ps(vD.v, ::PTVectorSIMDNegate(PTVectorSIMD{ vTemp1 }).v);
}

inline PTIntersectionType PT_VECTORCALL PTFrustumSIMDAABSIMDIntersect(PTFrustumSIMD F, PTAABSIMD AAB)
{
	__m128 AnyOutside;
	__m128 AllInside;

	__m128 OutSide;
	__m128 Inside;

	::PTSIMDInternalPlaneAABIntersect(F.Plane[0], AAB, Inside, OutSide);
	AnyOutside = OutSide;
	AllInside = Inside;

	::PTSIMDInternalPlaneAABIntersect(F.Plane[1], AAB, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	::PTSIMDInternalPlaneAABIntersect(F.Plane[2], AAB, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	::PTSIMDInternalPlaneAABIntersect(F.Plane[3], AAB, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	::PTSIMDInternalPlaneAABIntersect(F.Plane[4], AAB, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	::PTSIMDInternalPlaneAABIntersect(F.Plane[5], AAB, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	if (::_mm_movemask_ps(AnyOutside))
	{
		return PTDISJOINT;
	}
	else if (::_mm_movemask_ps(AllInside))
	{
		return PTCONTAIN;
	}
	else
	{
		return PTINTERSECT;
	}
}